/*
 * This is the code for the Honda card in the Hybrid Autorickshaw
 * project.
 * 
 * Functionality is to control the functionality for the Honda engine.
 * 
 * 
 * Functionality:
 * Fuel Level			I	PORTB1 PCINT1
 * Engine RPM			I	PC0 EINT3
 * Engine Start			O
 * Engine Stop			O	 
 * Battery Current		I	adcinput9:PORTC5 from adc obj
 * Battery Voltage		I	adcinput8:PORTC4 from adc obj
 *
 * Choke servo			O	PWM2A:PB0 
 * Throttle servo		O	PWM0A:PD0
 * Battery Temperature	I	adcinput6:PORTB5 From adc obj
 * Engine Temperature	I	adcinput5:PORTB2 from adc obj
 * 
 * @TODO Test everything, fix fuel level
 */

#include <tT.h>
#include <env.h>

#include "can.h"
#include "honda.h"
#include "adcinput.h"

#define HIGH	140
#define CENTER	94
#define LOW		47

#define MAXCHOKE 129
#define NOCHOKE 97

#define MAXTHROTTLE	74
#define MINTHROTTLE	114

#define STARTRPM 1000

honda_t honda = {tt_object(),0,0,0,0,0,0,0,MINTHROTTLE-1,0};

uint8_t startAttempts = 0;

void hondaCan(honda_t *self, can_message_t *msg);

can_listener_t listener_honda = {(tt_object_t*)&honda, (tt_method_t)hondaCan, ENV_MSEC(10), 0x600, 0x6F0};

void hondaInit(honda_t *self, void *none){
	//PCICR	|= (1<<PCIE0);
	//PCMSK0	= 0;
	
	//Full beam button
	DDRB	&= ~(1<<DDB1);
	PORTB	|= (1<<PINB1);
	//PCMSK0	|= (1<<PCINT1);
	
	//Fuel Level, Engine RPM
	DDRB		|=	(1<<DDB0);
	
	//DDRC	|=	(1<<DDC0);
	
	DDRD		|=	(1<<DDD1)|(1<<DDD0);
	PORTD		&= ~(1<<PIND0);
	
	//pc6,pb3,pb4.
	//Highside start for engine
	DDRC		=	(1<<DDC6);
	DDRB		|=	(1<<DDB3)|(1<<DDB4);
	
	//PWM SIGNAL
	POC			=	(1<<POEN2A)|(1<<POEN0A);	//Enable the PWM output for the servo
	POCR0SA		=	0;
	POCR0RA		=	self->dutyCycle; //Dutycycle for the throttle servo
	POCR2SA		=	0;
	POCR2RA		=	NOCHOKE;	//Dutycycle for the choke servo
	POCR0SB		=	1000;
	POCR2SB		=	1000;
	POCR_RB		=	1250;	//50 hz PWM signal
	PCNF		=	(1<<POPA);//(1<<PMODE);		//Center aligned PWM
	PCTL		=	(1<<PPRE1)|(1<<PPRE0)|(1<<PRUN);
	
	//RPM MEASURE
	EICRA		= (1<<ISC31)|(1<<ISC30);
	EIMSK		= (1<<INT3);
	
	//Enable the ADCS
	ADCinput.ADC9enabled = ON;
	ADCinput.ADC8enabled = ON;
	ADCinput.ADC6enabled = ON;
	ADCinput.ADC5enabled = ON;
	
	//Start the ADC sampling
	TT_ASYNC(&ADCinput,ADCRead,TT_ARGS_NONE);
	
	TT_ASYNC(self,fuelWarning, TT_ARGS_NONE);
	TT_ASYNC(self, speedCalc, TT_ARGS_NONE);
	
	can_listener_t * l = &listener_honda;
	TT_SYNC(&can, canInstall, &l);
}

/*
 * Fuel warning function
 * @TODO test the functionality
 */
void fuelWarning(honda_t *self, void *none){
	uint8_t savedPin = ~PINB;
	if((savedPin & 0x2) == 0x2){
		self->fuelWarning = 1;
		hondaCanSend(0x404);
	}else{
		self->fuelWarning = 1;
		hondaCanSend(0x405);
	}
	TT_AFTER(ENV_MSEC(1000),self,fuelWarning,TT_ARGS_NONE);
}

/*
* Function to measure the temperature off the battery
* What to do when overheated?
*/
void getBatteryTemperature(honda_t *self, void *none ){
	self->batteryTemperature = ADCinput.ADC6val;
	TT_AFTER(ENV_MSEC(2000),self,getBatteryTemperature,TT_ARGS_NONE);
}

/*
* Function to measure the engine temperature
* What to do when overheated?
*/
void getEngineTemperature( honda_t *self,void *none ){
	self->engineTemperature = ADCinput.ADC5val;
	TT_AFTER(ENV_MSEC(2000),self,getEngineTemperature,TT_ARGS_NONE);
}

/*
* Function to measure the voltage off the battery to check if we need to charge with the motor
*/
void getBatteryVoltage( honda_t *self, void *none ){
	//(1023/5)*(48/11) = 892,8
	//(1023/5)*(44/11) = 818,4
	//(1023/5)*(1/11) = V * 18,6
	self->batteryVoltage = ADCinput.ADC8val;
	TT_AFTER(ENV_MSEC(1000),self,getBatteryVoltage,TT_ARGS_NONE);
}

/*
* Function to measure the current off the battery to check if we need to charge with the motor
*/
void getBatteryCurrent( honda_t *self, void *none )
{
	//(1023/5)*(2,5+(0,625*300/100)) = 895,125
	self->batteryCurrent = ADCinput.ADC9val;
	TT_AFTER(ENV_MSEC(1000),self,getBatteryCurrent,TT_ARGS_NONE);
}

void chokeEngine(honda_t *self, void *none){
	POCR2RA = MAXCHOKE;
	TT_AFTER(ENV_MSEC(3000),self,stopChoke,TT_ARGS_NONE);
}

void stopChoke(honda_t *self, void *none){
	POCR2RA = NOCHOKE;
}

/*
* Function to measure the RPM to the generator
*/
void speedCalc(honda_t *self, void *none){
	uint16_t period;
	uint16_t rpm;
	can_message_t msg;
	
	ENV_DEBUG("Speedcalc\n\r");
	
	ENV_PROTECT(1);
	period = self->counter;
	self->counter  = 0;
	ENV_PROTECT(0);
	
	rpm = (period*5)>>1; // 2 = 60/24. 60 for minutes and 24 for the teeths
	
	self->engineRPM = rpm;
	if(self->engineRPM > STARTRPM)
		self->started = 1;
	else
		self->started = 0;
	*((uint16_t *) msg.data + 0) = rpm;
	msg.data[0] = rpm;
	msg.id = 0x771;
	msg.length = 2;
	//hondaCanSend(rpm>>1);
	TT_SYNC(&can, canSend, &msg);
	TT_AFTER(ENV_SEC(1), self, speedCalc, TT_ARGS_NONE);
}	

void startEngine(honda_t *self, void *none){
	if(!self->started){
		if(startAttempts > 2){
			TT_ASYNC(self,chokeEngine,TT_ARGS_NONE);
			if(startAttempts == 5)
				startAttempts = -1;	
		}
		TT_ASYNC(self,startStartEngine,TT_ARGS_NONE);
		//TT_AFTER(ENV_SEC(5),self,startEngine,TT_ARGS_NONE);
		startAttempts++;
	}else{
		startAttempts = 0;
	}
}	

//Counter for speed calculation, will get cleared from the speedCalc function
void speedInt(void){
	honda.counter++;
}

/*
* Function to start the starter engine
* @TODO Make sure that the motor starts from the RPM. If not we need to try again.
*/
void startStartEngine(honda_t *self, void *none){
	PORTC	=	(1<<PINC6);
	PORTB	=	(1<<PINB4);
	TT_AFTER(ENV_MSEC(1500),self,stopStartEngine,TT_ARGS_NONE); //Maybe make this go longer?
}

/*
* Function to stop the engine
*/
void stopStartEngine(honda_t *self, void *none){
	PORTC	&=	~(1<<PINC6);
	PORTB	&=	~(1<<PINB4);
}

/*
*Function to kill the engine
*/
void killEngine(honda_t *self, void *none){
	PORTB	|=	(1<<PINB3);
	TT_AFTER(ENV_MSEC(500),self,stopKillEngine,TT_ARGS_NONE);
}
/*
*Helper function to be able to start the engine later.
*/
void stopKillEngine(honda_t *self, void *none){
	PORTB	&=	~(1<<PINB3);
}	

/*
*Function to send messages to the can-bus
*/
void hondaCanSend(uint16_t msgId){
	can_message_t msg;
	msg.id = msgId;
	msg.length = 0;
	TT_SYNC(&can, canSend, &msg);
}

/*
* Function to handle the CAN messages for the card
*/
void hondaCan(honda_t *self, can_message_t *msg) {
	switch (msg->id & 0xF) {
		case 0x1:
			if(self->dutyCycle<MINTHROTTLE){//Increasing value gives an decreasing throttle
				self->dutyCycle++;
				POCR0RA	= self->dutyCycle;
			}
			break;
		case 0x2:
			if(self->dutyCycle>MAXTHROTTLE){//Decreasing value gives an increasing throttle
				self->dutyCycle--;
				POCR0RA	= self->dutyCycle;
			}
			break;
		case 0x3:
			TT_ASYNC(self, startEngine, TT_ARGS_NONE);
			break;
		case 0x4:
			TT_ASYNC(self, killEngine, TT_ARGS_NONE);
			break;
		case 0x5:
			POCR2RA = MAXCHOKE;
			break;
		case 0x6:
			POCR2RA = NOCHOKE;
			break;
		case 0x7:
			self->started = 1;
			break;
	}
	hondaCanSend(self->dutyCycle);
	hondaCanSend(POCR1RA);
}

ENV_INTERRUPT(INT3_vect,speedInt);
//ENV_INTERRUPT(PCINT1_vect, fuelWarning);